#include "motion/safezone.h"
#include <math.h>

//将当前点位转换至世界坐标系
void convertPointToWorld(const ZucPose* user_point, const ZucPose* current_user_offset, ZucPose* base_point)
{
    PmRpy user_offset_rpy;
    PmRotationMatrix user_offset_matrix;
    user_offset_rpy.y = current_user_offset->a * PM_PI / 180.0;
    user_offset_rpy.p = current_user_offset->b * PM_PI / 180.0;
    user_offset_rpy.r = current_user_offset->c * PM_PI / 180.0;
    pmRpyMatConvert(&user_offset_rpy, &user_offset_matrix);

    PmRpy user_point_rpy;
    PmRotationMatrix user_point_matrix;
    user_point_rpy.y = user_point->a * PM_PI / 180.0;
    user_point_rpy.p = user_point->b * PM_PI / 180.0;
    user_point_rpy.r = user_point->c * PM_PI / 180.0;
    pmRpyMatConvert(&user_point_rpy, &user_point_matrix);

    PmHomogeneous user_point_homogeneous;
    user_point_homogeneous.rot = user_point_matrix;
    user_point_homogeneous.tran = user_point->tran;

    PmHomogeneous offset_homogeneous;
    offset_homogeneous.rot = user_offset_matrix;
    offset_homogeneous.tran = current_user_offset->tran;

    PmHomogeneous result_homogeneous;
    pmHomHomMult(&offset_homogeneous, &user_point_homogeneous, &result_homogeneous);

    PmRpy current_base_rpy;
    pmMatRpyConvert(&result_homogeneous.rot, &current_base_rpy);

    base_point->tran = result_homogeneous.tran;
    base_point->a = current_base_rpy.y * 180.0 / PM_PI;
    base_point->b = current_base_rpy.p * 180.0 / PM_PI;
    base_point->c = current_base_rpy.r * 180.0 / PM_PI;
}

int checkSafePlane(SafePlane plane, ZucPose curPose, double* dist, int elbow_wrist_flag, double R_elbow, ZucPose current_user_offset)
{
    int ret = 0;
    double d;
    double safeD = 0.0;
    switch (elbow_wrist_flag)
    {
    case 0:
        safeD = plane.safeD;
        break;
    case 1:
        safeD = R_elbow + plane.safeD;
        break;
    default:
        break;
    }

    convertPointToWorld(&curPose, &current_user_offset, &curPose);
    *dist = plane.norm.point.x * curPose.tran.x + plane.norm.point.y * curPose.tran.y + plane.norm.point.z * curPose.tran.z + plane.d;
    d = plane.norm.point.x * plane.safePoint.point.x + plane.norm.point.y * plane.safePoint.point.y + plane.norm.point.z * plane.safePoint.point.z + plane.d;
    if (d > 0.0)
    {
        *dist = *dist - safeD;
    }
    else
    {
        *dist = *dist + safeD;
    }

    *dist = (d * (*dist)) / fabs(d);
    ret = (*dist) > 0 ? 0 : 1;  // on dangeous side
    return ret;
}
int checkSafeSphere(SafeSphere sphere, ZucPose curPose, double* dist)
{
    int ret = 0;
    PmCartesian d;
    pmCartCartSub(&sphere.center.point, &curPose.tran, &d);
    pmCartMag(&d, dist);
    if (*dist >= sphere.radius)
    {
        ret = 1;
    }
    return ret;
}
int checkSafeDiamond(SafeDiamond diamond, ZucPose curPose, double* dist)
{
    int ret = 0;
    PmCartesian center, d;
    if ((curPose.tran.x - diamond.diamondPoints[0].point.x) * (curPose.tran.x - diamond.diamondPoints[1].point.x) >= 0)
    {
        ret = 1;
    }
    else if ((curPose.tran.y - diamond.diamondPoints[0].point.y) * (curPose.tran.y - diamond.diamondPoints[1].point.y) >= 0)
    {
        ret = 1;
    }
    else if ((curPose.tran.z - diamond.diamondPoints[0].point.z) * (curPose.tran.z - diamond.diamondPoints[1].point.z) >= 0)
    {
        ret = 1;
    }
    else
    {
        ret = 0;
    }
    center.x = (diamond.diamondPoints[0].point.x + diamond.diamondPoints[1].point.x) * 0.5;
    center.y = (diamond.diamondPoints[0].point.y + diamond.diamondPoints[1].point.y) * 0.5;
    center.z = (diamond.diamondPoints[0].point.z + diamond.diamondPoints[1].point.z) * 0.5;
    pmCartCartSub(&center, &curPose.tran, &d);
    pmCartMag(&d, dist);

    return ret;
}

/**
	return : 0 : safe 	1:dangerous
**/
int checkSafezone(SafeZone zone,
                  ZucPose curPose,
                  ZucPose cur_elbow_Pose,
                  int elbow_wrist_flag,
                  double R_elbow,
                  ZucPose current_user_offset,
                  int pre_flag,
                  checksafefalg_out* checksafefalg)
{
    int i = 0, ret = 0;  // final decision to trigger a stop
    double delt_dist = 0.0;
    double delt_elbow_dist = 0.0;
    static double dist[6];
    static double last_dist[6];
    static double dist_elbow[6];
    static double last_dist_elbow[6];

    double pre_delt_dist = 0.0;
    double pre_delt_elbow_dist = 0.0;
    static double pre_dist[6];
    static double pre_last_dist[6];
    static double pre_dist_elbow[6];
    static double pre_last_dist_elbow[6];
    double safezone_eps = -1e-8;
    checksafefalg->checkSafezone = 0;
    checksafefalg->pre_checkSafezone = 0;
    //rtapi_print("[checkSafezone]: zone.type = %d  \n",zone.type);
    switch (zone.type)
    {
    case PLANE:
        for (i = 0; i < 6; i++)
        {
            if (zone.zone.plane[i].enable)
            {
                switch (pre_flag)
                {
                case 0: {
                    int trigStop = checkSafePlane(zone.zone.plane[i], curPose, &dist[i], 0, R_elbow, current_user_offset);
                    int trigStop_elbow = 0;
                    if (elbow_wrist_flag == 1)
                        trigStop_elbow = checkSafePlane(zone.zone.plane[i], cur_elbow_Pose, &dist_elbow[i], elbow_wrist_flag, R_elbow, current_user_offset);
                    if (zone.safeZoneMode == 0)
                    {
                        delt_dist = dist[i] - last_dist[i];
                        // rtapi_print("dist = %f  \n", dist[i]);
                        trigStop = trigStop && (delt_dist < safezone_eps);  // out of safety zone and move away from safety zone
                        if (elbow_wrist_flag == 1)
                        {
                            delt_elbow_dist = dist_elbow[i] - last_dist_elbow[i];
                            trigStop_elbow = trigStop_elbow && (delt_elbow_dist < safezone_eps);
                            last_dist_elbow[i] = dist_elbow[i];
                        }
                        ret = ret || trigStop || trigStop_elbow;
                        last_dist[i] = dist[i];
                    }
                    else
                    {
                        ret = ret || trigStop || trigStop_elbow;
                    }
                    if (ret)
                        checksafefalg->checkSafezone = 1;
                    break;
                }
                case 1: {
                    int trigStop = checkSafePlane(zone.zone.plane[i], curPose, &pre_dist[i], 0, R_elbow, current_user_offset);
                    int trigStop_elbow = 0;
                    if (elbow_wrist_flag == 1)
                        trigStop_elbow = checkSafePlane(zone.zone.plane[i], cur_elbow_Pose, &pre_dist_elbow[i], elbow_wrist_flag, R_elbow, current_user_offset);
                    if (zone.safeZoneMode == 0)
                    {
                        pre_delt_dist = pre_dist[i] - pre_last_dist[i];
                        // rtapi_print("dist = %f  \n", dist[i]);
                        trigStop = trigStop && (pre_delt_dist < safezone_eps);  // out of safety zone and move away from safety zone
                        if (elbow_wrist_flag == 1)
                        {
                            pre_delt_elbow_dist = pre_dist_elbow[i] - pre_last_dist_elbow[i];
                            trigStop_elbow = trigStop_elbow && (pre_delt_elbow_dist < safezone_eps);
                            pre_last_dist_elbow[i] = pre_dist_elbow[i];
                        }
                        if (curPose.tran.x == 0 && curPose.tran.y == 0 && curPose.tran.z == 0 && curPose.a == 0 && curPose.b == 0 && curPose.c == 0)
                            trigStop = 0;
                        ret = ret || trigStop || trigStop_elbow;
                        pre_last_dist[i] = pre_dist[i];
                    }
                    else
                    {
                        ret = ret || trigStop || trigStop_elbow;
                    }
                    if (ret)
                        checksafefalg->pre_checkSafezone = 1;
                    break;
                }
                default:
                    break;
                }
            }
        }
        break;
    case SPHERE:
        for (i = 0; i < 3; i++)
        {
            if (!zone.zone.sphere[i].enable)
            {
                //ret = 0;
                continue;
            }
            ret = checkSafeSphere(zone.zone.sphere[i], curPose, &dist[i]);
            if (!zone.safe)
            {
                ret = !ret;
                break;
            }
        }
        break;
    case DIAMOND:
        for (i = 0; i < 3; i++)
        {
            if (!zone.zone.diamond[i].enable)
            {
                //ret = 0;
                continue;
            }
            ret = checkSafeDiamond(zone.zone.diamond[i], curPose, &dist[i]);
            if (!zone.safe)
            {
                ret = !ret;
                break;
            }
        }
        break;
        // default:
        // 	rtapi_print(" [checkSafezone] : no such safe zone type %d \n",zone.type);
        // 	break;
    }

    return ret;
}

int checkSafezone_dangerous(SafeZone zone, ZucPose curPose, ZucPose cur_elbow_Pose, int elbow_wrist_flag, double R_elbow, ZucPose current_user_offset)
{
    int i = 0, ret = 0;  // final decision to trigger a stop
    double dist[6];
    double dist_elbow[6];
    //rtapi_print("[checkSafezone]: zone.type = %d  \n",zone.type);
    switch (zone.type)
    {
    case PLANE:
        for (i = 0; i < 6; i++)
        {
            if (zone.zone.plane[i].enable)
            {
                int trigStop = checkSafePlane(zone.zone.plane[i], curPose, &dist[i], 0, R_elbow, current_user_offset);
                int trigStop_elbow = 0;
                if (elbow_wrist_flag == 1)
                    trigStop_elbow = checkSafePlane(zone.zone.plane[i], cur_elbow_Pose, &dist_elbow[i], elbow_wrist_flag, R_elbow, current_user_offset);
                ret = ret || trigStop || trigStop_elbow;

                // rtapi_print("[checkSafezone]: plane[%d].enable = %d  \n",i,zone.zone.plane[i].enable);
            }
        }
        break;
    case SPHERE:
        for (i = 0; i < 3; i++)
        {
            if (!zone.zone.sphere[i].enable)
            {
                //ret = 0;
                continue;
            }
            ret = checkSafeSphere(zone.zone.sphere[i], curPose, &dist[i]);
            if (!zone.safe)
            {
                ret = !ret;
                break;
            }
        }
        break;
    case DIAMOND:
        for (i = 0; i < 3; i++)
        {
            if (!zone.zone.diamond[i].enable)
            {
                //ret = 0;
                continue;
            }
            ret = checkSafeDiamond(zone.zone.diamond[i], curPose, &dist[i]);
            if (!zone.safe)
            {
                ret = !ret;
                break;
            }
        }
        break;
        // default:
        // 	rtapi_print(" [checkSafezone] : no such safe zone type %d \n",zone.type);
        // 	break;
    }

    return ret;
}